/*
 * MultiPIDController.h
 *
 *  Created on: Jan 15, 2011
 *      Author: user
 */

#ifndef MULTIPIDCONTROLLER_H_
#define MULTIPIDCONTROLLER_H_



#include "Base.h"
#include "semLib.h"
#include <vector>

class PIDOutput;
class PIDSource;
class Notifier;

/**
 * Class implements a PID Control Loop that takes readings from multiple data sources, and
 * sums their weighted outputs.
 *
 * Creates a separate thread which reads the given PIDSource and takes
 * care of the integral calculations, as well as writing the given
 * PIDOutput
 */
class MultiPIDController
{
private:
	std::vector<float> m_P;			// factor for "proportional" control
	std::vector<float> m_I;			// factor for "integral" control
	std::vector<float> m_D;			// factor for "derivative" control
	std::vector<float> m_maximumOutput;	// |maximum output|
	std::vector<float> m_minimumOutput;	// |minimum output|
	std::vector<float> m_maximumInput;		// maximum input - limit setpoint to this
	std::vector<float> m_minimumInput;		// minimum input - limit setpoint to this
	std::vector<bool> m_continuous;	// do the endpoints wrap around? eg. Absolute encoder
	bool m_enabled; 			//is the pid controller enabled
	std::vector<float> m_prevError;	// the prior sensor input (used to compute velocity)
	std::vector<double> m_totalError; //the sum of the errors for use in the integral calc
	std::vector<float> m_tolerance;	//the percetage error that is considered on target
	std::vector<float> m_setpoint;
	std::vector<float> m_error;
	std::vector<float> m_result;
	float m_finalResult;
	std::vector<float> m_weight;
	float m_weightSum;
	float m_period;

	SEM_ID m_semaphore;

	std::vector<PIDSource*> m_pidInput;
	PIDOutput *m_pidOutput;
	Notifier *m_controlLoop;

	static void CallCalculate(void *controller);
	void Calculate();
	DISALLOW_COPY_AND_ASSIGN(MultiPIDController);
	
public:
	MultiPIDController(float p, float i, float d, float weight,
					PIDSource *source, PIDOutput *output,
					float period = 0.05);
	~MultiPIDController();
	int addPID(float p, float i, float d, float weight, PIDSource *source);
	int removePID(int);
	float Get();
	void SetContinuous(bool continuous , int n);
	void SetInputRange(float minimumInput, float maximumInput, int n);
	void SetOutputRange(float mimimumOutput, float maximumOutput , int n);
	void SetPID(float p, float i, float d, int n );
	float GetP(int n);
	float GetI(int n);
	float GetD(int n);

	void SetSetpoint(float setpoint,int n);
	float GetSetpoint(int n);

	float GetError(int n);

	void SetTolerance(float percent, int n);
	bool OnTarget();

	void Enable();
	void Disable();

	void Reset();
};


#endif /* MULTIPIDCONTROLLER_H_ */
